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This  research  program  has  been  the  final  extension  of  a  previous  research  gram  that 
was  completed  on  August  31,  1988  and  was  reported  in  the  final  report  dated  September 
12,  1988.  The  work  described  here  is  the  final  part  of  the  effort  covering  the  period  from 
September  1988  to  September  31,  1989. 

The  work  since  September  of  1988  has  resulted  in  the  following  developments: 

•  scientific  methods  for  generation  of  robot  trajectories  that  can  deliver  maximum 
impulse  at  the  end-point  of  the  trajectory. 

•  design  of  the  world  modeling  system  RWORLD  (Robot  WORLD)  for  CAD 
Based  robot  programming  and  simulation. 

•  final  development  and  completion  of  the  computer  software  OPTLOAD  for  cal¬ 
culation  of  load  optimal  robot  trajectories. 

•  initial  work  in  the  development  of  a  theoretical  basis  for  world  model  calibration 
based  on  features. 

The  results  of  the  above  developments  have  been  or  are  being  published  in  the  fol¬ 
lowing  technical  publications: 

A.  PUBLISHED 

1 .  Ravani,  B.,  "World  Modeling  for  CAD  Based  Robot  Programming  and  Simulation," 
Int'l  Journal  of  Robotics  and  Automation,  Vol.  4,  No.  2,  1989,  pp.  96-105. 

B.  PLANNED  PUBLICATIONS 

1.  Wang,  L.T.  and  B.  Rc'ani,  "Maximum  Impulse  Robot  Trajectories,"  Technical 
Note,  to  be  submitted  to  the  ASME  Journal  of  Dynamic  Systems,  Measurements  and 
Control. 


2.  Ravani,  B.,  S.  Semen,  and  H.  Van  Brussel,  "World  Model  Calibration  in  Robot 
Simulation,"  to  be  submitted  to  Journal  of  Manufacturing  Systems. 


The  available  set  of  these  publications  (A.l)  or  their  theoretical  basis  (B.2)  is  given 
in  Appendix  A.  These  publications  are  in  addition  to  the  eight  technical  publications 
described  in  the  Final  Report  of  12  September  1988. 


A  method  has  been  developed  that  would  allow  generation  of  robot  trajectories  that 
can  deliver  maximum  impulse  at  a  specified  end-point  of  their  trajectory.  Since  impulse  can 
be  related  to  the  dynamic  payload,  the  method  for  generation  of  load  optimal  trajectories 
developed  earlier  has  been  modified  to  handle  the  problem.  The  resulting  scheme  has  ap¬ 
plications  in  military  applications  of  robotics  involving  automatic  loading  of  weapon 
systems.  In  such  a  situation  proper  engagement  of  the  ammunition  in  certain  weapons 
require  an  impulsive  motion  from  the  robot. 

In  the  area  of  CAD  based  simulation,  the  initial  design  of  a  world  modeling  system 
has  been  completed.  The  system  referred  to  as  RWORLD  (Robot  WORLD)  is  designed 
using  our  experience  base  from  the  development  of  the  off-line  robot  simulation  system 
STAR.  RWORLD,  however,  advances  the  state  of  the  art  in  CAD  based  robot  program¬ 
ming  and  simulation  in  several  areas.  The  system  can  support  interfaces  to  the  actual  robot 
workcell  environment  allowing  for  calibration  of  the  workcell  for  inaccuracies.  The  system 
models  geometrical,  kinematical,  relational  and  physical  properties  of  the  world,  allowing 
for  geometrical  and  spatial  reasoning  as  well  as  reasoning  about  the  mechanics  of  manipu¬ 
lation.  It  also  supports  simulation  of  several  sensory  functions  and  multiple  arm  coordi¬ 
nated  control. 

The  initial  design  of  the  RWORLD  has  also  been  modified  to  be  able  to  perform 
world  model  calibration  based  on  geometric  features.  This  is  the  first  time  that  world 
mouei  calibration  is  addressed  from  a  feature  level  point  of  view.  Furthermore,  the  addi¬ 
tion  of  a  feature  primitive  to  the  data  structure  of  RWORLD  has  opened  np  ?  new  direction 
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of  research  in  feature  based  programming  and  simulation.  This  together  with  expansion  of 
RWORLD  is  part  of  the  basis  for  our  newly  funded  ARO  project. 
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WORLD  MODELING  FOR  CAD-BASED 
ROBOT  PROGRAMMING  AND  SIMULATION 

Bahram  Ravani  (*) 


Abstract 

The  design  of  a  world  modeling  system  for  CAD-based  robot 
programming  and  simulation  is  presented.  The  system  can  support 
interfaces  to  actual  robot  workcell  environments,  allowing  for  calibra¬ 
tion  of  the  workcell  model  for  inaccuracies  and  its  use  for  robot  con¬ 
trol.  The  system  models  geometrical,  spatial,  relational,  and  physical 
properties  of  the  world,  allowing  for  geometrical  and  spatial  reason¬ 
ing  as  well  as  reasoning  about  the  mechanics  of  manipulation.  It 
supports  simulation  of  several  sensory  functions  and  multiple-arm 
coordinated  control.  It  also  supports  representation  of  assemblies 
and  aggregation  of  multiple  devices.  The  system  design  presented 
is  the  basis  of  a  world  modeling  system  for  model-based  robot  task 
planning,  simulation,  and  control  currently  under  development. 

Key  Words 

Geometric  Modeling.  Robotic  Programming,  CAD/CAM 

1.  Introduction 

World  modeling  is  the  process  of  providing  a  com¬ 
puter  representation  of  a  workcell  in  robotic  applications. 
The  world  mollel  provides  the  necessary  knowledge  of  the 
robot  workcell  for  a  task  planner  to  generate  appropriate 
robot  motion  control  commands.  The  task  planner  ac¬ 
cesses  the  world  model  when  converting  implicit  or  explicit 
motion  commands  into  trajectory  directives,  and  updates 
the  world  model  when  the  state  of  the  workcell  configura¬ 
tion  changes  During  robot  task  execution,  for  example, 
parts  are  grasped,  repositioned,  and  released  or  assemblies 
a.-v  created.  All  this  must  be  reflect e-d  within  the  world 
model 

In  CAD-based  robot  programming,  the  world  model 
includes  CAD  models  of  the  robot  and  its  workcell  envi¬ 
ronment.  If  the  programmer  performs  reasoning  for  task¬ 
planning  purposes  by  interacting  with  a  computer  graphics 
I  CADI  display  of  the  robot  and  its  environment  (generated 
from  the  data  in  the  world  model),  the  programming  or 
the  task-planning  process  is  called  interactive  or  off-line 
(see  for  example.  [1.2]).  On  the  other  hand,  if  a  computer 
program  (using  the  data  in  the  world  model)  performs  the 
reasoning  for  task  planning,  the  process  is  called  automatic 
robot  task  planning  and  programming  (see  for  exan  pie. 
[3.4]). 

t  This  paper  was  first  published  in  NATO  ASI  Series  F.  Vol.  50, 

C A  D-Based  Programming  for  Sensor  Robots,  ed  B.  Ravani. 

Springer  Verlag.  1988 

(*)  Department  of  Mechanical  Engineering,  University  of  Califor¬ 
nia  -  Davis.  Davis.  CA  95616  USA 


This  paper  describes  the  design  of  the  world  modeling 
system  RWORLD  (Robot  WORLD),  which  is  the  basis  of 
a  CAD  based  (off-line)  robot  programming  and  simulation 
environment  presently  under  development  in  our  labora¬ 
tory.  In  contrast  to  its  predecessor  off-line  planning  sys¬ 
tem  STAR  (;oj).  RWORLD  has  real  time  interfaces  to  the 
actual  robot  workcell  both  for  correcting  inaccuracies  in 
the  world  model  as  well  as  for  model  based  robot  control. 
The  system  has  several  advanced  capabilities  providing  for 
an  inactive  environment  for  robot  task  planning,  simula¬ 
tion.  and  control.  It  models  geometrical  (CAD),  relational, 
and  physical  (kinematic  and  dynamic)  properties  of  the 
world.  These  allow  for  geometrical  and  spatial  reasoning 
as  well  as  reasoning  about  the  mechanic  of  manipulation. 
RWORLD  is  also  structured  to  support  parallelism  (paral¬ 
lel  execution)  for  simulation  of  multiple  cooperative  robot 
arms  operating  at  the  same  time.  Furthermore,  it  allows 
definition  of  aggregation  and  assemblies  to  handle  simula¬ 
tion  of  tool  changes  and  creation  of  component  assemblies. 
It  also  uses  sensory  processing  for  world  model  calibration 
eliminating  or  reducing  modeling  errors.  Finally,  the  sys¬ 
tem  provides  functions  for  geometric  simulation  of  sensory 
interactions  allowing  emulation  of  eiror  recovery  during 
interactive  task  planning. 

Each  of  the  foregoing  features  is  described  in  more 
detail  in  subsequent  sections. 

2.  Modeling  of  Robotic  Workcell  Environments 

In  off-line  planning  and  simulation,  the  simulator  re¬ 
quires  an  appropriate  modei  of  the  robot  world.  In  robotic 
control,  the  planner  also  accesses  the  same  world  model. 
The  world  model  should  include  a  geometrical  as  well  as  a 
physical  description  of  the  world.  It  should  also  describe 
tii-  relationships  between  different  entities  in  the  world. 
The  geometrical  description  is  needed  for  geometrical  rea¬ 
soning  and  the  physical  description  is  needed  for  reasoning 
about  the  mechanics  of  manipulation.  The  relational  de¬ 
scription  allows  for  spatial  and  temporal  reasoning  and 
planning. 

In  a  complex  application,  a  manipulator  may  use  sev¬ 
eral  different  tools  and  end-effectors,  interact  with  other 
equipment  or  manipulators,  and  use  sensory  data  and  pro¬ 
cess  parts  to  create  component  assemblies  or  products. 
The  tools  or  end-effectors  used  may  be  other  complex 
manipulators  such  as  dexterous  mechanical  hands  The 
parts  may  also  be  subassemblies  of  other  parts.  This 
means  that  robot  workcells  consist  in  hierarchical  or  non- 
hierarchical  aggregations  of  several  distinct  elements  such 
as  parts,  equipment,  and  sensors,  each  having  different 
physical  representations  A  rigid  part,  for  example,  docs 
not  have  any  internal  kinematic  representation  while  a 
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manipulator  does  have  an  interna!  kinematic  representa¬ 
tion  due  to  the  relative  freedom  between  its  links.  For  this 
reason,  RWORLD  uses  a  multi-primitive  representation 
of  the  workcell  environment  at  an  abstract  level.  These 
abstract  primitives  are  devices,  rigid  objects  (objects  for 
short),  frames,  and  sensors.  There  are  also  a  set  of  affix- 
ment  descriptors  representing  the  relation  between  these 
primitives. 

A  device  is  an  entity  having  one  or  more  internal  de¬ 
grees  of  freedom.  A  device  can  affect  ■  -hi-  ms  within  its 
workspace  by  imparting  motion  to  them,  exerting  a  force 
on  them,  or  processing  them.  Examples  of  devices  arc- 
robots.  machine  tools,  conveyor  belts,  end -effectors,  and 
flexible  fixtures.  A  rigid  object  is  an  entity  with  no  inter¬ 
nal  degree  of  freedom  but  that  is  capable  of  being  moved 
with  six  degrees  of  freedom  in  the  workcell  environment. 
Examples  of  objects  are  workpieces  and  tables.  The  two 
primitives  of  devices  and  objects  are  defined  in  a  hierar¬ 
chical  fashion  such  that  objects  can  be  accessed  by  dev  ices 
but  devices  cannot  be  accessed  by  objects.  This  is  be¬ 
cause  objects  can  be  processed  or  manipulated  by  devices 
but  devices  cannot  be  manipulated  or  ptccessed  by  ob¬ 
ject®  Frames  are  the  third  primitive  in  the  world  model: 
they  are  used  to  mark  a  location,  i.e..  a  position  and  ori¬ 
entation  in  space.  Such  frames  are  referred  to  as  natural 
frames.  In  addition  to  their  use  as  one  of  the  basic  elements 
of  the  world  model,  frame  primitives  are  also  used  to  mark 
feature  locations  on  objects  or  devices.  Such  frames  are  re¬ 
ferred  to  as  auxiliary  frames.  Frames  indicating  target  lo¬ 
cations  in  space  can  also  be  considered  as  auxiliary  frames 
for  the  free  space.  Sensors  are  the  last  primitive  within 
the  world  model.  A  sensor  primitive  provides  a  functional 
representation  for  sensory  interactions  for  simulation  pur¬ 
poses.  RWORLD  design  facilitates  emulation  of  touch, 
tactile,  proximity,  and  presence-sensing  functions. 

The  sensor  primitive  has  the  same  Irerarchical  level  as 
the  object  primitive  within  the  data  structure:  it  can  there¬ 
fore  be  attached  to  devices  and  objects.  Frames  are  in  the 
lowest  level  of  hierarchy.  They  can  be  attached  to  them¬ 
selves.  objects  or  sensors,  and  devices.  Fig  .1  illustrates 
the  hierarchical  organization  of  these  four  primitives. 


DEVICE 


FRAME 


Figure  1.  The  hierarchical  organization  of  RWORLD 
primitives 


The  four  primitives  of  frames,  objects,  sensors,  and 
devices  form  the  basis  for  spatial,  geometrical,  relational, 
and  physical  representation  of  the  world.  RWORLD  sup¬ 
ports  multiple  representation  of  these  primitives  at  geo- 
nietrical  and  physical  levels  to  speed  up  the  computations 
ne.'-ssary  at  different  1-v-b  of  robot  task  planning,  simu¬ 
lation.  and  programming. 

3.  Spatial  and  Geometrical 
Representation  of  the  World 

The  spatial  and  geometrical  representation  of  tit- 
robot  workcell  can  be  described  by  assigning  certain  own¬ 
ership  properties  for  the  four  abstract  primitives  of  frame;, 
objects,  sensors,  and  devices.  A  frame  primitive  is  then 
represented  by  a  name  and  will  own  a  homogeneous  trans¬ 
formation  matrix  and  a  C'AD  descriptor.  The  transforma¬ 
tion  matrix  describes  the  spatial  relationship  between  the 
frame  and  a  global  coordinate  frame  in  the  workcell,  and 
the  CAD  descriptor  provides  a  geometric  or  shape  repre¬ 
sentation  of  the  frame.  This  representation  is  in  the  form 
of  three  mutually  orthogonal  axes.  A  frame  primitive  can 
oe  instantiated  based  on  its  spatial  relationship  descriptor 
a  homogeneous  transformation  matrix).  Fig.  2  illustrates 
the  structure  of  the  frame  primitive.  Instantiation  of  a 
frame  primitive  allows  definition  of  relative  frames. 

A  rigid  object  primitive  is  represented  by  a  name  a 
natural  frame  representing  the  spatial  relationship  of  the 
object  relative  to  another  frame,  and  a  set  of  properties 
such  as  mass,  inertia,  and  center  of  mass  location.  The 
center  of  mass  location  is  specified  with  respect  to  the 
natural  frame  of  the  object.  The  set  of  properties  can  also 
include  other  characteristics  of  the  objects  such  as  ma¬ 
terial  properties  needed  in  modeling  interactions  between 
objects  or  surface-  reflectance  and  other  properties  needed 
for  vision -coat rolled  manipulation.  The  information  on 
mass,  inertia,  and  center  of  mass  location  allow  for  multi- 
pi-  physical  representation  of  an  object  in  terms  of  both 
a  rigid  body  model  as  well  as  a  more  si  in;  iified  lumped 
mas>  m-'d-l.  I  hi?,  dual  representation  simplifies  some  of 
tli-  computation'  n-c-ssary  when  mas.  nir.g  about  or  sim¬ 
ulating  tie  m-  :.  tin  ■'  of  manipuia!  loi. 


Name 


t 


Relative  Frames 


Figure  2.  The  structure  of  the  frame  primitive  and  its 
instances. 
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An  object  primitive  (Fig.  3)  owns  a  CAD  descriptor 
providing  its  geometric  (shape)  representation  and  possi¬ 
bly  one  or  more  auxiliary  frames  indicating  feature  loca¬ 
tions  on  the  object.  RWORLD  uses  a  polyhedral  solid 
modeling  system  for  its  geometric  or  CAD  representation. 
Furthermore,  it  supports  two  instances  of  CAD  descrip¬ 
tion.  one  in  terms  of  the  polyhedral  model  and  one  in  terms 
of  the  more  simplified  bounding  parallelpiped  (bounding 
be  x  1  or  prismatic  representation. 

An  object  can  also  he  instantiated  based  on  the  homo¬ 
geneous  transformation  matrix  associated  with  its  natural 
frame.  Different  instances  of  an  object,  in  this  context, 
then  represent  different  locations  of  the  same  object  in 
the  workspace  or  different  references  to  the  same  object 
in  terms  of  relative  frames.  These  frames  are  usually  rep¬ 
resented  either  with  respect  to  the  natural  frame  of  the 
object  or  with  respect  to  other  auxiliary  frames  represent¬ 
ing  the  more  basic  features  of  the  same  object. 


Model  Model  Repres.  of  Features 


A  sensor  primitive  (Fig.  4)  is  represented  by  a  name,  a 
type  descriptor,  and  a  natural  frame:  it  owns  a  geometric 
CAD  descriptor  The  type  descriptor  indicates  whether 
the  primitive  represents  a  touch,  a  tactile,  a  proximity, 
or  a  presence  sensor.  The  natural  frame  represents  the 
spatial  relationship  of  the  sensor  in  the  workceil.  Within 
RWORLD.  sensors  are  modeled  geometrically  in  t-  rni'  of 
prismatic  objects  only.  This  means  that  tie  p  i.-  on!\  n- 
instance  of  the  CAD  descriptor  for  a  sen-m. 

The  representation  of  the  device  primitive  i>  mom 
complicated  than  the  other  primitives,  since  T  vices  have 
internal  degrees  of  fre^cm  and  are  programmable  or  re¬ 
quire  control  statements.  A  device  primitive  (Fig.  o,1  is 
represented  by  a  name,  a  natural  frame,  a  mobility  de¬ 
scriptor.  and  a  set  of  joints  and  links.  It  owns  a  set  of 
motion  programming  commands  and  a  Tool  Center  Point 
(TCP).  The  natural  frame  is  used  to  represent  the  spatial 
relationship  between  the  base  of  the  device  and  another 
frame  within  the  workcell.  The  mobility  descriptor  speci¬ 
fies  the  number  of  internal  degrees  of  freedom  of  the  device. 
The  joints  and  links  are  lower  level  primitives  residing  sub¬ 
ordinate  to  a  device  primitive. 

A  joint  primitive  of  a  device  is  represented  by  a  name, 
the  joint  number  within  the  device  (the  present  design  of 
RWORLD  can  only  model  devices  made  of  serial  kinematic 
chains),  and  a  set  of  joint  trajectory  limits.  These  limits 
indicate  the  bounds  on  joint  displacement,  velocity,  and 
acceleration  limits  as  well  as  limits  on  the  torque  (or  force) 
provided  at  the  joint  by  the  actuators.  Such  information  is 
needed  to  reason  about  the  mechanics  of  joint  motion.  The 
joint  primitive  owns  a  set  of  joint  trajectories  consisting  of 
joint  displacement,  velocity,  and  acceleration.  It  also  owns 
a  CAD  descriptor  for  computer  graphic  display  purposes. 


Figure  3  The  object  primitive  and  its  instances. 
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Figure  4  The  sensor  primitive  and  its  instances 


Figure  5.  The  device  primitive  and  its  instances 


98 


A  link  primitive  of  a  device  contains  a  link  name,  the 
link  number  within  the  device,  a  natural  frame,  and  a  set 
of  link  properties,  including  link  mass,  inertia,  and  center 
of  mass  location.  The  primitive  owns  a  geometric  CAD  de¬ 
scriptor  and  possibly  one  or  more  auxiliary  frames  repre¬ 
senting  features  on  the  link.  The  geometric  representation 
of  a  link  also  has  two  instances,  one  in  terms  of  a  polyht- 
dral  model  and  one  in  terms  of  a  prismatic  approximation 
cf  the  polyhedral  representation. 

The  TCP  of  a  device  is  an  auxiliary  frane-  represent¬ 
ing  a  coordinate  system  on  the  last  link  of  the  robot,  in 
terms  of  which  the  motion  program  of  the  robot  is  spec¬ 
ified.  The  set  of  motion-programming  comands  is  spec¬ 
ified  in  terms  of  this  frame,  either  by  an  explicit  robot  - 
programming  language  or  by  synthesis  from  implicit  task- 
level  specification  of  a  robot  operation.  Instantiation  of 
the  coordinate  transformation  of  the  TCP  in  terms  of  other 
feature  frames  of  the  device  allows  definition  of  multiple 
tool  center  points.  The  set  of  motion-programming  com¬ 
mands  specifies  the  manipulation  function  of  a  device  in 
carrying  out  a  task.  Different  instances  of  this  set  repre¬ 
sent  different  tasks  that  are  to  be  completed  by  a  device. 

The  specifications  of  the  four  primitives  of  frames,  ob¬ 
jects,  sensors,  and  devices  provide  for  a  complete  represen¬ 
tation  of  the  geometry  and  spatial  relationships  between 
the  elements  of  a  robotic  workcell  environment. 

4.  Physical  Representation  of  the  World 

The  physical  representation  of  a  robot  workcell 
(world)  should  emulate  kinematics  as  well  as  dynamics  of 
objects  and  devices  in  the  workcetl  environment.  It  should 
also  include  models  for  gravity,  friction  between  surfaces, 
and  interactions  between  various  elements  of  t lie  workcell. 
This  is  so  because  robotic  manipulation  involves  imparting 
motion  to  objects  as  well  as  applying  forces  and  moments 
to  them. 

The  representation  of  mass,  inertia  properties,  and 
center  of  mass  location  for  cbj-<*ts  fas  discussed  in  th* 
previous  section)  facilitates  physical  modeling  of  obp-’ts 
using  Newton's  laws  of  motion  The  development  of  phys¬ 
ical  (kinematic  and  dynamic  I  models  of  devices  however 
is  slightly  more  involved  due  to  the  relative  decrees  of  free¬ 
dom  between  their  links. 

A  key  consideration  for  physical  modeling  of  devices 
is  the  computational  efficiency  of  the  resulting  formulation 
necessary  for  simulation  purposes.  The  multi-link  nature 
of  a  device  such  as  a  robot  suggests  the  use  of  a  recursive 
computational  formalism.  RWORLD  automatically  gener¬ 
ates  kinematic  and  dynamic  models  for  devices  in  a  form 
that  would  allow  compulations  with  forward  or  backward 
recursions.  This  provides  enough  flexibility  in  simulating 
physical  models  of  devices  in  real  time. 

Fig.  6  is  a  skeleton  diagram  of  a  six-degree  -of- 
freedom  robot  manipulator  with  the  natural  frames  of  its 
links  numbered  from  one  to  six  In  this  figure,  the  seventh 
coordinate  frame  represents  the  TCP  of  the  robot  The 
kinematic  equations  for  this  device  describe  the  spatial 
relationship  between  the  seventh  and  the  first  coordinate 
systems  in  terms  of  the  joint  varicbles  of  the  robot.  These 


equations  can  be  derived  recursively  by  considering  the 
spatial  relationship  between  the  coordinate  systems  of  two 
adjacent  links. 

Fig.  7  shows  the  ith  and  the  (i  —  l)th  coordinate  sys¬ 
tems  connected  through  the  ith  link.  Let  (x;y.z;)(j  = 
l.i  —  li  be  s  set  of  three  orthogonal  unit  vectors  coinci- 
ient.  respectively,  with  the  axes  of  the  ith  and  the  ti-*-l  ith 
coordinate  systems.  Since  these  unit  vectors  form  a  dex- 
tral  set.  two  of  them  completely  define  the  orientation  of 
tie-  corresponding  coordinate  system  From  th<‘  geornM ry 
in  Fig.  7.  one  ran  write  fi’ij: 

X.4.;  =  x,  cos  9,  —  y,  sin  ill 

and 

z,_;  =  z,  cos  a,  -1-  (x,  +  i  X  z, )  sin  n,  (2) 

Substituting  from  ( 1)  into  (2).  t his  last  equation  simplifies 


z,4-i  =  z,  cos  q,  4-  (x,  sin  6,  —  y,  cos  9 , )  sin  a,  (3) 

Equations  i  and  3  form  a  set  of  two  forward  recursive 
formulas  for  calculating  the  orientation  of  the  (i  +  l)th 
coordinate  system  in  terms  of  that  of  the  ith  coordinate 
system. 

Similarly,  a  pair  of  backward  recursive  formulas  can 
be  derived  as 

z,  =  z1  +  i  cos  a,  +  y>  +  1  sin  a,  (4) 


x,  =  x, 4. ]  cos  6,  t-  (z,  +  i  sin  a,  -  >  ,  +  !  cos  a,  1  sin  9,  (5) 


Figure  6.  Skeleton  diagram  of  a  robot  with  natural  coor¬ 
dinate  systems  of  its  various  links. 


Figure  7.  Two  neighboring  joints  of  a  robot. 
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Repeated  use  of  thise  last  two  equations  allow  computa¬ 
tion  of  the  orientation  of  the  TCP  coordinate  system  with 
respect  to  the  base  or  first  coordinate  system.  The  set  of 
two  equations  (1)  and  (3)  or  (4)  and  (5)  each  require  17 
multiplications  and  three  additions  plus  four  trigonometric 
evaluations  for  their  computations.  The  forward  recursive 
formulas  perform  the  computations  from  the  ah  coordi¬ 
nate  system  to  the  (n  —  1  )t h  coordinate  system  (ri  is  the 
number  of  degrees  of  freedom  of  the  device).  The  back¬ 
ward  formulas,  however,  compute  from  the  ith  coordinate 
system  to  the  base  coordinate  system. 

For  most  commercially  available  manipulators,  the 
link  twist  angles.  u,’s.  are  either  0C  or  90°  The  result¬ 
ing  simplified  equations  are: 

For  forward  recursion: 

With  q,  =  0°: 

x,,!  =  x,  cose,  •+•  y,  sinfq;  z,*.;  =  z,  *6) 

With  a,  =  90°: 

x,  +  i  =  x,  cos  8,  -s  y,  sin  Q,z,+i  = 

x,  sin  8,  -  y,  cos  0,  (7) 

For  backward  recursion: 

With  a,  =  0°: 

X,  =  x1  +  i  cos  8,  =  y,+1  sin  0,  ,  z,  =  z1  +  1  (8) 

With  q,  =  90°: 

x,  =  x.4-1  cos  6,  j-  z,  +  1  sin  8,  :  z,  =  y1  +  i  (9) 

The  spatial  relationship  between  the  it  —  l)th  and  :th 
coordinate  system  also  requires  computation  of  position 
of  the  origui  of  one  coordinate  system  with  respect  to  the 
other.  The  position  of  the  origin  of  the  |i  —  1  )th  tuo-di- 
nate  system  with  respect  to  the  ith  coordinate  system  is 
specified  by  a  position  vector  r"  (Fig.  7b  This  vector  can 
be  written  in  terms  of  the  link  length  a.  and  the  joint 
offset  s.  as 

r,"  =  -■>, z,  —  a, x.^j  i  It*! 

Now  if  -  r,  represent  the  position  vector  from  the  origin 
of  the  ith  coordinate  system  to  that  of  the  .ith  coordinate 
system,  then  t lie  forward  recursive  formula  for  the  compu¬ 
tation  of  the  position  of  a  coordinate  system  becomes 

;r,.i  =  ;r,  =  r'  ill' 

with  -  r,  =  0 

and  r*  is  given  in  equation  (10)  For  backward  recursion 
th-  corresponding  equation  is 

Jr,  =;r,+;  +r;  (12) 

Equations  11  and  12  each  require  six  multiplications  and 
two  additions. 

Repeated  use  of  the  recursive  formulas  for  position 
and  orientation  allows  computation  of  full  kinematics  of 


a  robot  manipulator  or  spatial  relationships  between  two 
coordinate  frames  related  through  a  set  of  other  auxiliary 
or  natural  frames. 

In  addition  to  the  kinematic  equations,  reasoning 
about  the  mechanics  of  manipulation  requires  mod’  line  of 
quasi -static  for'*'-/ torque  transmission  '  Lara  fa 

robot  as  well  n>  ■'•...tnp«n.t!i”ii  •  f  mv-rs-  -  f 

manipulator  Such  information  i>  ai-  ■  n-  b  f.  :  •  r  i 

purposes  if  tie-  r,  I-,,  i  j>  under  for'--  and  .  r  dunam:-  n- 
trol. 

Even  when  ;;i<'  r  ■ ! ,  • . ;  is  n  ;  >;;i  !■  r  n’r  . 

the  inverse  dynamo-  ••  -inputa!  k  :i>  ar-  :•  :  •  :•  a.-  >u 

about  the  TCP  path,  t  raversa!  rdo  :  n.  : 1  -v- 

cle  times  that  will  avoid  •.■•verloa.iing  ■  f  t  o  •  .•■:  > 

Sucli  computations  are  also  necessary  for  i -  id  -ana  -  j:  v  d— 
terininat. oti  7  and  planning  oi  optimal  t ra •••'!-.  r:-s.  l  ie 
computation  of  quasi- static  fore-  transmission  chara-ter- 
lstics  of  a  manipulator  allows  reasoning  a: -out  ti,-  f.  r--- 
( torques)  that  are  to  1-e  applied  !•••  w.  Tkpi-'e-  m  f,.r  ••  -  :n- 
trol  applications. 

RWORLD  uses  an  algorithm  that  computes  the  in¬ 
verse  dynamics  and  the  quasi-static  force  transmission 
characteristics  of  the  manipulator  simultaneously  The  tat- 
*  ,-r  is  determined  by  the  computation  of  the  so-calied  Ja¬ 
cobian  matrix  for  the  manipulator  [6].  The  inverse  dynam¬ 
ics  allow  computation  of  joint  torques  (forces)  necessary  to 
achieve  a  desired  TCP  velocity  and  acceleration.  The  Ja¬ 
cobian  matrix  allows  computation  of  the  .joint  torques  for 
forces)  necessary  to  apply  a  desired  set  of  forces  and/or 
torques  at  the  TCP  frame.  The  computational  scheme 
used  in  RWORLD  uses  the  backward  kinematic  equations 
together  with  the  Newton-Euler  formulation  of  manipula¬ 
tor  dynamics 

The  total  inertia  f->roe  and  inert ia  moment  exerted  nn 
•lie  ;th  link  of  a  manipulator  "in  be  com;  m- :  ms:  • 
t  ively .  from  i  l  ig  '  i 

F.  =  ui. V,. 


N,  —  l’:-  *  .  •  *  W  .  —  ~  ‘  W .  —  *  V. 

wii-r-  ~w  and  "  w  nr-  r- a  a:  .  .  .*  •.  - 
locity  and  angular  a  •  •  !•  rat i  :.  f  ,i:ik  w;  r- -•  •  •  : 
me  ! ;  —  1  H It  nvwing  <w.-r  imat-  system  *  i-  ;;i-  ;;  a  3 


Figure  8.  Forces  and  moments  exerted  on  tie  ••:(  l-.uk 
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inertia  matrix  of  link  i  evaluated  about  its  mass  center 
and  referenced  to  its  natural  frame  (i.e.,  the  ( i  +  1 ) th  co¬ 
ordinate  system).  The  angular  velocity.  1  +  1w,,  and  the 
angular  acceleration.  ,  +  1w,  are  computed  from 


'~:w,  =  r,  +  1^+1]Tw.:  ,  +  1w,  =  ["  +  1*,  +  l]T w,  (lo) 


=  ;x„:y,Tlz<4,] 

and  for  a  rotational  joint 

w.  =  w,_i  ~  z ,qt:  w,  =  w,.t  J-  w,_ixz,<7,  +  ztq,  (16) 

For  a  translational  joint,  these  equations  simplfy  to  where 
q,  s  are  the  joint  positions  (for  revolute  joints  q,  =  9,). 


w,  =  wt_;:  w,  =  w,_i  (if  ) 

The  linear  acceleration  of  the  mass  center  of  link  i  (in 
Equation  13)  is  computed  from 

V%,  =  w,  x  c,  -  w,  x  (w,  x  c.)  +  V,  (18) 
where  for  a  rotational  joint 

V,  =  Vj_ !  4-  w,  x  (w,  x  r*)  +  w,  x  r‘ 
but  for  a  translational  joint 


V,  =  Vj_t  +  w,x  r'  -v-  w,  x  (w,  x  r*)  +  2w,  x  z,q,  +  z,q, 

the  relationships  and  the  constraints  existing  between  var¬ 
ious  elements  of  the  world  should  also  be  properly  repre¬ 
sented  and  maintained.  RWORLD  uses  affix  nent  descrip¬ 
tors  and  ownership  relations  for  relational  representation 
of  the  world. 

The  spatial  or  kinematic  relationships  between  vari¬ 
ous  elements  ar»  represented  by  the  coordinate  transfor¬ 
mation  matrices  owned  by  the  natural  frame  of  each  ele¬ 
ment.  The  hierarchical  relationships  between  vario  ele¬ 
ments  are  described  by  the  hierarchical  relationships  be¬ 
tween  the  primitives  and  between  auxiliary  frames.  All 
these  were  described  in  the  previous  sections.  This  section 
discusses  the  representation  of  affixments  or  connect i  ities 
between  different  primitives  making  up  the  world  model. 

Affixment  descriptors,  within  RWORLD.  describe  a 
form  of  ownership  relations  that  cannot  be  instantiated 
but  can  be  defined  and  modified  dynamically  during  pro¬ 
gramming  and  execution.  Each  affixment  descriptor  das 
a,  attribute  that  describes  the  nature  of  the  affixment  or 
connectivity  of  two  primitives  within  the  world  model.  At 
present  two  types  of  attributes  are  used  to  represent,  re¬ 
spectively.  rigid  and  non-rigid  affixments.  The  rigid  affix¬ 
ment  is  used  to  describe,  for  example,  grasping  of  parts  by- 
end-effectors  or  fixtures,  assemblies,  and  device  aggrega¬ 
tions.  The  rigid  affixment  is  fully  supported  in  RWORLD. 


The  non-rigid  affixment  is  used  to  describe  the  situa¬ 
tion  where  one  item  rests  on  another  item.  The  non-rigid 
attribute  is  only  partially  supported  within  RWORLD  to 
describe  situations  where  objects  or  devices  are  placed,  in 
stable  locations,  on  the  top  of  other  objects  or  devices.  An 
example  is  one  block  that  is  placed  on  top  of  a  larger  block. 
Furthermore,  the  non-rigid  affixment  only  represents  the 
nature  of  the  interdependence  of  two  elements  of  the  world 
from  a  purely  kinematic  viewpoint.  In  other  words,  if  an 
object  R  is  affixed  to  an  object  A  by  a  non-rigid  affixment. 
then  if  object  A  undergoes  a  displacement,  object  R  also 
undergoes  the  same  displacement. 

The  converse  of  this  situation,  however,  does  not  hold. 
In  this  process,  it  is  assumed  that  the  motion  of  A  neither 
change  the  relative  position  of  object  B  with  respect  to  .  b- 
ject  A  nor  does  it  make  object  B  unstable  relative  to  object 
A.  Ideally,  the  non-rigid  affixment  should  also  model  the 
nature  of  contact  and  force  transmission  between  the  con¬ 
tacting  surfaces  of  the  two  objects. 

The  use  of  rigid  affixments  allows  creation  of  assem¬ 
blies  by  contacanating  objects.  It  also  allows  creation  of 
device  aggregates  at  run  time  without  the  need  to  change 
or  remodel  the  two  aggregated  devices  as  one  device.  This 
is  specially  important  in  simulating  tool  changes  where, 
for  example,  a  robot  picks  up  a  dexterous  hand.  In  such 
a  situation  the  new  robot-dexterous-hand  combination  ac¬ 
tually  represents  a  new  device  with  a  totally  different  set 
of  kinematic  and  dynamic  equations. 

In  RWORLD,  since  the  kinematic  and  dynamic  equa¬ 
tions  for  ‘he  robot  and  the  dexterous  hand  are  already 
developed  in  the  world  model,  the  rigid  affixment  (or  ag¬ 
gregation)  of  the  twe  is  handled  by  considering  the  two 
devices  under  coordinated  control.  The  parallel  command 
execution  technique  described  in  the  next  section  is  then 
used  to  program  the  motion  of  the  robot-dexterous-hand 
combination  by  separately  using  the  corresponding  kine¬ 
matic  and  dynamic  equations  in  a  coordinated  manner. 

When  using  affixments.  the  relationships  between  dif- 
feren  components  of  a  robot  workcel!  can  be  described 
in  the  form  of  a  tree  or  a  graph  structure.  At  present. 
RWORLD  only  supports  the  tree  structure.  The  nodes  of 
a  tree  are  data  elements  describing  one  of  the  four  primi¬ 
tives  of  devices,  ob  jects,  sensors  and  frames  Fig.  9  shows 
a  workcell  where  two  dexterous  hands  are  aggregated  to 
a  robot  and  the  robot  is  aggregated  to  a  conveyor  belt, 
which  itself  has  one  translational  degree  of  freedom  and 
represents  another  device. 

In  addition,  there  are  two  objects,  each  grasped  by  one 
cf  the  two  dexterous  hands,  and  with  one  of  ‘he  objects 
representing  an  assembly  of  three  components.  There  are 
also  two  auxiliary  frames  affixed  to  this  assembly.  A  tree 
representation  of  this  workceli  is  shown  in  Fig.  10. 

As  the  rigid  affixments  are  deleted  during  program 
run  time,  the  tree  is  split  into  smaller  trees  at  the  cor¬ 
responding  nodes.  This  is  the  case,  for  example,  when 
the  end-effectors  elease  the  objects  they  are  holding  or  if 
the  robot  performs  a  tool  change  replacing  the  two  end- 
effectors  by  other  end-of-arm  toolings.  The  ownership  re¬ 
lationship  represented  by  affixment  descriotors  allows  up- 
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Figure  9.  A  robot  with  aggregated  end-effectors  before 
and  after  grasping  a  part  and  an  assembly. 


Figure  10  The  tree  structure  of  the  workcell  hierarchy 
of  Figure  9  after  the  robot  has  grasped  the  part  and  the 
assembly 


dating  the  location  of  all  nodes  of  the  tree  subordinate 
to  the  node  whose  location  in  the  workcell  is  changed. 
Crude  versions  of  some  of  the  device  aggregation  ideas 
were  also  implemented  in  the  STAR  program  using  con¬ 
ventional  programming  languages  such  as  “c"  [£].  These 
implementations,  however,  did  not  use  parallel  operation 
of  aggregated  devices.  RWORLD  supports  both  serial  and 
parallel  execution  of  devices  aggregated  or  operated  coop- 
erativelv. 


5.  Handling  of  Parallelism 


Parallelism,  in  the  context  of  CAD-based  simulation 
of  robotic  workcells,  is  when  multiple  devices  are  operating 
at  the  same  time.  The  devices  may  be  aggregated  or  work¬ 
ing  in  parallel  in  a  cooperative  manner.  In  such  situations 
the  robot  workcell  simulation  environment  should  be  able 
to  prope'ly  display  parallel  operations  of  these  devices. 
This  can  be  handled  by  first  allowing  parallel  and  serial 
aggregation  of  memory  segments  allocated  to  each  device 
during  execut  ion.  Then  if.  for  example,  parallel  processors 
are  used,  each  parallel  memory  aggregate  can  be  allocated 
to  a  different  processor;  otherwise,  the  execution  should  be 
swapped  between  parallel  aggregates.  In  this  latter  case, 
the  time  increments  for  the  execution  of  the  motion  pro¬ 
grams  for  devices  that  are  working  in  parallel  should  be 
controlled. 

Since  certain  devices  are  operated  in  series  with  oth¬ 
ers.  and  the  motion  programs  for  all  parallel  devices  may- 
end  at  different  time  steps,  a  device  checker  should  also 
be  used.  The  function  of  the  device  checker  is  to  check 
the  aggregation  and  execution  status  of  each  device.  This 
is  done  during  execution,  either  to  schedule  operation  of 
a  device,  if  it  is  serially  operated  with  respect  to  certain 
other  devices,  or  to  neutralize  a  device  from  parallel  opera¬ 
tion  when  its  task  is  compietd.  The  device  checker  also  in¬ 
forms  the  time-step  controller  about  the  devices  for  which 
the  execution  time  should  be  sliced  into  small  increments. 
The  time-step  controller  then  swaps  the  execution  between 
these  devices  such  that  a  uniform  execution  time  between 
all  the  devices  is  created.  This  is  all  illustrated  schemat¬ 
ically  in  Fig.  11.  Fig.  12  is  a  computer  generated  display 
of  three  devices  (two  robots  and  a  convey  belt)  operated 
in  parallel  to  perform  a  material  transfer  and  an  ajnembly 
task. 
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Figure  11.  Tlie  execution  environment  for  handling  paral¬ 
lelism. 


Figure  12  Computer  display  of  parallel  operation  of  de¬ 
vices. 


G.  World  Model  Calibration 

In  CAD-based  robot  programming  and  simulation, 
uncertainties  are  introduced  due  to  modeling  errors  and 
tolerances  on  geometrical  descriptions  and  specification  of 
spatial  relationships  between  objects.  These  errors  are  in 
addition  to  those  introduced  by  sensors  and  actuators  dur¬ 
ing  task  execution.  The  latter  set  of  errors  are  control  er¬ 


rors  and  can  be  corrected  by  sensory  control  during  task 
execution.  The  first  set  of  errors,  however,  are  errors  in 
the  world  model.  These  errors  may  be  corrected  by  world 
model  calibration.  Calibration  uses  sensory  information 
to  update  the  world  model. 

Most  previous  studies  on  robot  calibration  r'i'  have 
been  devoted  to  robot  programmin':  > y  s  using  sim¬ 

plistic  world  models.  These  systems  <■:..}  u>i'  the  robot 
kinematic  equations  as  part  of  the  world  in  ■  ivi  and.  in  ad¬ 
dition.  are  not  supported  by  a  Simula; !■.•:.  •  nvir  nnient  or 
a  geometric  modeling  system.  Furtherin'  "  .  regular  robot 
calibration  procedures  only  involve  calibration  of  internal 
robot  model  and  not  all  the  components  of  the  robot  work¬ 
cell.  Calibration  of  all  the  components  of  tin-  world  model 
of  a  robotic  workccll  requires  sensory  interactions  between 
the  world  mode!  and  the  robot  workcell. 

Grossman  and  Taylor  [10]  have  described  a  sys¬ 
tem  where  a  robot  itself  is  moved  to  a  position  within 
its  workspace  to  update  position  or  orientation  data. 
Hasegawa  [11]  has  developed  a  system  that  can  be  used 
to  construct  and  calibrate  position  data  as  well  as  to  ver¬ 
ify  geometric  shape  models  in  an  interactive  fashion.  More 
recently,  Ishii  et  al.  [12]  have  developed  a  sensor  that  can 
ease  up  the  sensing  of  position  and  orientation  data  in  a 
robot  workcell  environment. 

In  RWORLD,  we  are  developing  a  system  that  uses 
a  combination  of  tactile  and  vision  sensing  for  monitoring 
the  workcell.  Once  sensory  data  is  obtained  from  a  robot 
workcell,  it  is  used  to  modify  the  world  model.  In  CAD- 
based  programming,  calibration  is  usually  performed  be¬ 
fore  the  robot  program  is  downloaded  to  the  actual  robot. 
This  means  that  the  already-generated  robot  program  may 
also  have  to  be  modified  due  to  the  modifications  of  the 
world  model.  In  order  to  make  the  robot  program  inde¬ 
pendent  of  the  numerical  data  associated  with  the  robot 
workcell.  RWORLD  uses  the  abstract  primitive  represen¬ 
tation  of  the  workcell  in  terms  of  devices,  objects,  sensors, 
and  frames. 

The  numerical  lata  on  spatial  relationships  an  J  phys¬ 
ical  and  shape  ’properties  are  ail  kept  in  separat-  files  that 
can  be  accessed  in  ti."  corresponding  prtmit  If  the 
robot  program  is  tie::  written  symbol: Till;.  ::.  onus  of 
these  primitives,  up  tat*-  of  the  nunv'ri":  vaoe-  ttt  the 
world  model  does  not  require  any  changes  in  tin-  robot 
program:  it  only  requires  the  update  of  numerical  val¬ 
ues  in  the  corresponding  numerical  data  files  Further¬ 
more.  since  present  robot-programn ing  languages  do  not 
support  many  of  the  details  associated  with  shape  infor¬ 
mation  or  physical  properties  of  the  world  only  calibra¬ 
tion  of  spatial  (or  kinematic)  relationships  is  considered 
in  R\\  ORLD.  The  system,  however  is  designed  to  sup¬ 
port  more  higher  level  calibrations  lor  shape  or  physical 
properties. 

7.  Simulation  of  Sensory  Interactions 

In  CAD-based  simulation  of  sensors  robots  the  world 
model  should  be  able  to  model  sensory  interactions  that 
may  occur  between  the  robot  and  its  workccll  environment 
This  is  necessary  in  order  to  allow  reasoning  about  error 
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recovery  in  programming  robots  equipped  with  external 
sensors.  There  are  two  main  categories  of  externa]  sensing 
techniques,  one  involving  local  and  the  other  using  global 
interactions  with  the  environment.  The  existing  sensing 
functions  for  the  first  category  include  touch,  proximity, 
force,  and  presence  sensing.  Vision  is  probably  the  most 
important  external  sensory  function  involving  global  (long- 
range)  interaction  with  the  robot  environment. 

In  a  CAD-based  robot  simulation  system,  simulation 
of  local  sensing  functions  involves  interactions  with  geo¬ 
metric  models  of  objects.  This  means  that  all  such  sensory 
functions  can  be  simulated  geometrically.  RWORLD  uses 
interference  checking  between  geometric  models  of  sensors 
and  objects  to  simulate  touch,  tactile,  presence,  and  prox¬ 
imity  sensing  functions.  Existence  of  interference  between 
the  geometrical  model  of  a  sensor  primitive  representing  a 
touch  sensor  and  that  of  an  object  simulates  existence  of 
contact  between  the  two  elements.  This  is  very  simple  and 
Wats  suggested  by  Meyer  [13]. 

Meyer  also  suggested  the  use  of  simple  interference 
checking  to  simulate  the  use  of  a  light  beam  between  the 
fingers  of  an  end-effector  to  check  presence  or  absence  of 
objects.  In  this  case,  the  geometric  model  of  the  sen¬ 
sor  should  represent  the  geometry  of  the  light  beam.  If, 
in  addition  to  touch,  distribution  of  contact  (tactile  in¬ 
formation)  is  also  to  be  simulated,  then  the  problem  is 
slightly  more  complicated.  RWORLD  partitions  the  ge¬ 
ometric  model  of  the  tactile  array  into  an  array  of  small 
geometrical  segments  and  then  performs  the  interference 
checking  between  each  segment  and  the  object.  All  seg¬ 
ments  having  interferences  with  the  object  are  then  color 
coded  for  display  of  distribution  of  contact  (Fig.  13). 

The  simulation  of  proximity  sensing  is  also  very  sim¬ 
ilar  to  this  and  again  should  use  interference  checking  for 
segmented  models.  In  thir  case  the  geometrical  model  of 
the  sensor  consists  of  a  prismatic  object  having  the  same 
size  as  the  volumetric  range  of  the  actual  priximity  sensor 
being  simulated  (Fig.  13).  This  geometric  representation 
of  the  volumetric  range  of  the  sensor  is  partitioned  into 
segments,  each  representing  units  of  proximity  to  the  ob¬ 
ject  to  which  the  actual  sensor  is  attached.  The  proximity 


Tigure  13.  Geometric  partitioning  for  simulation  of  tactile 
and  proximity  sensing. 


sensing  function  can  then  be  simulated  by  checking  for  in¬ 
terferences  between  the  segments  of  the  sensor  model  and 
the  external  object.  The  segments  having  interferences 
with  the  object  are  then  color  coded  for  display  purposes 
to  i  1 1  us* rat e  the  remaining  units  of  r !<  >s>  n'->s  to  the  object . 

Simulation  of  the  sensory  fun."  :■  n-  d'-'n!"  I  s  ,  far 
requires  algorithms  for  fast  d<M<-ti..n  ■  n-.  -  be¬ 

tween  geometric  (solid)  models.  This  i-  why  RWOIILL) 
uses  prismatic  approximation  of  polylj.-- jra!  soli  is  in  the 
second  level  of  its  multi-level  repro'-nt  at  t  i.  f  'in.  s  re¬ 
shape  information.  Interference  bet  w.--»  prismatic  solids 
can  be  computed  faster  than  that  of  polyhedral  '  lids. 

A  higher  level  of  complication  arts".'  if  for.  -  sensing 
is  to  be  simulated  geometrically.  Tn  this  case  volume  of 
overlap  between  the  geometric  modi'!  of  t h--  sensor  and 
the  object  should  be  computed  in  addition  to  checking  for 
the  existence  of  overlap  or  interference.  Different  amounts 
of  overlan  should  also  be  calibrated  in  relation  to  different 
levels  of  force  readings  in  the  actual  force  sensor. 

Simulation  of  global  sensory  interactions  with  the  en¬ 
vironment  such  as  that  of  vision  has  not  yet  been  consid¬ 
ered  for  RWORLD. 

8.  Conclusion 

This  oaper  has  discussed  the  basic  elements  of  world 
modeling  for  CAD-based  robot,  programming  and  simula¬ 
tion.  The  design  of  an  advanced  world  modeling  system, 
RWORLD.  is  also  presented  together  with  examples  from 
partial  implementations  of  some  of  the  elements  of  the  sys¬ 
tem.  The  overall  design  of  RWORLD  is  very  open  ended 
and  flexible.  Its  implementation  can  grow  to  a  powerful 
but  generic  system  that  can  form  the  basis  for  robotic  sim¬ 
ulation .  programming,  and  control. 
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Introduction 


In  CAD-based  off-line  programming  and  simulation,  it  is  necessary  to  have  an  appropriate 
model  of  the  robot  world.  The  world  model  provides  the  necessary  knowledge  of  the 
robot  workcell  for  a  task  planner  to  generate  appropriate  robot  motion  control  commands. 
The  task  planner  accesses  the  world  model  when  convening  motion  commands  into 
trajectory  directives  and  updates  the  world  model  when  the  state  of  the  workcell 
configuration  changes.  During  robot  task  execution,  for  example,  parts  are  grasped, 
repositioned  and  released  or  assemblies  are  created.  All  this  must  be  reflected  within  the 
world  model. 

Up  to  this  moment,  existing  CAD-based  off-line  programming  and  simulation  systems  only 
work  with  nominal  world  models.  Robot  programs,  called  nominal  robot  programs,  are 
created  based  upon  these  nominals  models.  However  uncertainties  exist  in  the  physical 
world,  uncertainties  whose  values  cannot  be  exactly  determined  at  the  time  the  nominal 
world  model  and  robot  program  are  created.  Even  if  one  is  able  to  measure  and  determine 
the  uncertainties  on  a  specific  moment,  it  is  possible  that  new  uncertainties  will  be 
introduced  before,  or  during  robot  program  execution. 

There  are  three  major  sources  of  uncertainties:  (1)  the  manipulator,  (2)  the  objects  to  be 
manipulated,  and  (3)  the  introduction  of  these  objects  into  the  work  environment. 

To  eliminate  or  reduce  the  second  and  third  source  of  uncertainties,  the  nominal  world 
model  should  be  calibrated  before  robotprogram  execution.  This  means  that  w-e  have  to 
define  the  real  location  of  a  number  of  frames  in  the  world  model.  The  location  of  each 
object  or  device  in  the  world  model  is  defined  by  a  special  frame,  named  the  natural  frame 
of  the  object  or  device,  so  w'e  can  deal  with  the  third  source  of  uncertainties  by  defining 
the  real  location  of  these  natural  frames.  Special  characteristics  of  the  objects  (grasp 
locations,  holes,  ...)  are  represented  by  auxiliary  frames.  Defining  the  real  location  of  these 
frames  eliminates  or  reduces  the  second  source  of  uncertainties. 

Tnc  first  source  of  errors  can  be  dealt  with  in  different  ways.  First,  one  can  calibrate  the 
kinematic  model  of  the  robot  before  robot  program  execution.  Secondly,  it  should  be 
possible  to  execute  the  robotprogram  only  once  with  sensory  feedback  (e.g.  force.)  to 
determine  the  errors  caused  by  the  imperfect  kinematic  model  of  the  manipulator.  Thirdly, 
the  robot  program  execution  can  be  done  with  on-iine  sensory  feedback  (e.g.  external  force 
control)  to  deal  with  these  errors.  The  last  two  possibilities  have  the  extra  advantage  that 
they  aiso  reduce  the  errors  caused  by  unknown  uncertainties  or  uncertainties  introduced 
during  execution. 

In  this  work  the  errors  introduced  by  the  imperfect  kinematic  model  of  tnc  manipulator  will 
be  reduced  by  using  the  second  or  third  method.  Tnc  first  method  can  form  pan  of  future 
research  and  can  then  be  compared  with  results  obtained  by  the  two  other  methods. 

Tne  first  pan  of  this  work  will  consist  :n  developping  a  world  model  calibration  procedure. 
Tms  procedure  will  be  implemented  in  ne  world  modeling  system  R WORLD.  As  we  will 
see  further  RWORLD  uses  a  muld-pnminve  representation  of  the  workcell  environment. 
These  pnmidves  are  devices,  ngid  objects,  frames  and  sensors.  To  make  world  model 
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calibration  possible,  we  have  to  introduce  a  new  primitive:  the  feature  primitive.  It  is  this 
feature  primitive  that  will  be  measured  in  the  physical  world.  The  measurement  of  the 
features  will  be  only  based  on  point  measurements. 

The  global  procedure  for  creating  a  robot  pregram,  including  calibration  of  the  world 
model,  can  be  briefly  outlined  as  follows: 

1.  create  a  world  model,  consisting  of  devices,  objects,  sensors  and  frames 

2.  create  a  frame  based  robot  program 

3.  introduce  features  into  the  world  model  and  condition  the  location  of  important  frames 
upon  these  features 

4.  create  a  world  model  calibration  procedure 

5.  measure  necessary  features  with  point  measurements 

6.  update  world  model  and/or  robot  program 

7.  execute  robot  program  without  on-line  sensory  feedback 
or 

7.  execute  robot  program  with  on-line  sensory  feedback 
or 

7.  execute  robot  program  once  with  sensory  feedback,  update  robot  program,  execute 
robot  program  again  without  sensory  feedback 

Step  1  and  2  are  classic  steps,  also  occurring  in  existing  systems.  Step  3  is  new  and  will 
be  explained  later.  Step  4  can  be  viewed  as  the  creation  of  a  list  with  measuring 
directives:  which  features  have  to  be  measured?  Step  5-7  are  self-explanatory.  When  we 
say  "create"  in  step  1,  2  and  4  we  mean:  create  interactively  with  the  system.  However 
research  should  be  done  to  automate  the  4th  step.  Enough  information,  generated  by  the 
third  step  should  be  available  to  do  this  automation. 

Introducing  the  feature  primitive  for  world  model  calibration  opens  the  door  to  robot 
programming  on  feature  level.  Today  robot  programming  is  done  on  frame  level.  Motion 
commands  are  generated  based  on  frame  locations.  Research  should  be  done  to  facilitate 
the  robot  programming  by  reasoning  on  feature  level. 

Finally  a  sensor  device  should  be  designed  that  enables  us  to  do  point  measurements  in 
space. 
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In  a  complex  application,  a  manipulator  may  use  several  different  tools  and  end-effectors, 
interact  with  equipment  or  manipulators,  use  sensory  data  and  manipulate  and  process  parts 
to  create  component  assemblies  or  products.  The  tools  or  end-effectors  used  may  be  other 
complex  manipulators  such  as  dexterous  mechanical  hands.  The  parts  may  also  be 
subassemblies  of  other  parts.  This  means  that  robot  workcells  consist  of  hierarchical 
aggregations  of  several  distinct  elements  such  as  parts,  equipment  and  sensors  each  having 
different  physical  representations.  A  rigid  part,  for  example  does  not  have  any  internal 
kinematic  representation  while  a  manipulator  does  have  an  internal  kinematic  representation 
due  to  the  relative  freedom  between  its  links.  For  this  reason,  RWORLD  uses  a  multi¬ 
primitive  representation  of  the  workcell  environment  at  an  abstract  level.  These  abstract 
primitives  are  devices,  rigid  objects,  frames  and  sensors.  The  following  figure  illustrates 
the  hierarchical  organization  of  these  four  primitives: 


A  device  is  an  entity  having  one  or  more  internal  degrees  of  freedom.  Examples  of 
devices  are  robots,  machine  tools,  conveyor  belts,  end-effectors  and  flexible  fixtures.  A 
rigid  object  is  an  entity  with  no  internal  degrees  of  freedom  but  capable  of  being  moved 
with  six  degrees  of  freedom  in  the  workcell  environment.  Examples  of  objects  are 
workpieces  and  tables.  The  two  primitives  of  devices  and  objects  are  defined  in  a 
hierarchical  fashion  such  that  objects  can  be  accessed  by  devices  but  devices  cannot  be 
accessed  by  objects.  Frames  are  the  third  primitive  in  the  world  model:  They  are  used  to 
mark  a  location  (position  and  orientation)  in  space.  Each  device,  object  and  sensor  owns  a 
frame,  referred  to  as  natural  frame,  that  represent  its  location  in  space.  In  addition  to  their 
use  as  natural  frames,  frame  primitives  are  also  used  to  mark  characteristic  locations  on 
objects  or  devices.  Such  frames  are  referred  to  as  auxiliary  frames.  Sensors  are  the  last 
primitive  within  the  world  model.  A  sensor  primitive  provides  a  functional  representation 
for  sensory  interactions  for  simulation  purposes. 
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2.  The  feature  primitive 

The  calibration  of  the  nominal  world  model  consists  in  defining  the  real  locations  of 
important  natural  and  auxiliary  frames  in  the  world  model.  Therefore  measurements  have 
to  be  carried  out  on  the  physical  robot  workcell.  As  stated  in  the  introduction,  at  this 
moment  we  only  allow  point  measurements.  Future  research  may  investigate  other  possible 
measurement  methods.  Whatever  method  of  measurement  is  used,  it  will  always  be  subject 
to  errors.  Therefore  a  least  squares  approximation  approach  will  be  utilized  to  minimize 
these  errors. 

The  location  of  the  natural  frame  of  an  object  in  3D-space  is  totally  defined  by  the 
position  of  three  non-collinear  points  on  the  object,  so  we  would  have  enough  information 
to  find  the  real  location  by  measuring  three  points  on  the  object.  To  eliminate  or  reduce 
the  errors  introduced  by  the  measurement  of  the  three  points,  we  measure  more  than  three 
points  and  do  a  least  squares  approximation.  The  next  question  that  arises,  is  which 
multiple  points  to  measure.  Because  we  are  dealing  here  with  CAD-based  off-line 
programming,  we  can  assume  that  we  have  a  geometric  model  of  the  object.  So  we  can 
assume  that  we  know  the  equations  of  several  features  (surfaces,  curves,  ...)  of  the  object 
relative  to  the  object’s  natural  frame.  Therefore  we  will  measure  multiple  points  on 
features  and  we  will  define  the  equations  of  these  features  with  a  least  squares 
approximation  approach  to  reduce  the  measurement  errors. 

So  to  calibrate  a  nominal  world  model,  we  will  only  perform  point  me  asurements  on 
features.  These  point  measurements  will  enable  us  to  define  the  real  equations  of  the 
features  relative  to  the  world  frame  and  the  real  equations  of  these  features  will  enable  us 
to  find  the  real  locations  of  natural  and  also  of  auxiliary  frames.  It  is  this  information,  the 
real  locations  of  a  number  of  frames,  that  we  are  looking  for  in  world  model  calibration, 
because  up  to  this  moment  robot  programming  is  based  on  frame  information,  not  on 
feature  information. 

To  define  the  real  locations  of  these  important  frames  by  point  measurements  on  features 
wt  have  to  connect  these  frames  to  features.  A  frame  will  be  connected  to  a  feature  by  a 
condition.  The  feature  will  as  such  condition  the  frame  te.g.  the  (X.Y /-plane  of  the  frame 
has  to  be  parallel  to  a  plane  feature).  Therefore,  a  feature  primitive,  which  is  a  geometric 
characteristic  of  an  object  or  a  device  and  is  owned  by  that  object  or  device,  will  be 
introduced  into  the  hierarchical  organization  of  RWQRLD  primitives: 
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device 


A  frame  can  be  connected  to  a  feature  which  imposes  a  condition  on  the  frame.  In  that 
case  the  frame  and  the  feature  have  to  belong  to  the  same  object  or  device. 


3.  Defining  the  equations  of  features  by  point  measurements 

To  define  the  equations  of  features  we  will  perform  point  measurements  on  these  features. 
To  eliminate  or  reduce  the  measurement  errors,  wc  will  do  a  least  squares  approximation. 

In  this  chapter  we  will  discuss  the  plane  feature. 

In  many  physical  and  statistical  investigations  it  is  desirable  to  represent  a  system  of  points 
in  space  bv  the  "best-fitting"  plane.  Analytically  this  consists  in  taking 

z  =  a<,  ■*-  a,x  +  a;-y 

where  x.  y  and  z  are  variables,  and  determining  the  "best"  values  for  the  constants  a,,.  a; 
and  a:  in  relation  to  the  observed  corresponding  values  of  the  variables.  In  nearly  all  cases 
dealt  with  in  text-books  of  least  squares,  the  variables  on  the  right  of  our  equations  are 
treated  as  the  independent  those  on  the  left  as  the  dependent  variables.  In  many  cases 
however,  as  in  our  case,  the  "independent"  variable  is  subject  to  just  as  much  deviation  or 
error  as  the  "dependent”  variable.  In  these  cases,  we  will  consider  as  the  "best  fit",  the 
plane  that  minimizes  the  sum  of  the  squares  of  the  perpendiculars  from  the  system  of 
points  upon  the  plane. 

So  let  P„  P2 .  P„  be  the  system  of  points  with  coordinates  (x,.  y„  z.),  (x2.  y2,  Zj) . 

(x„,  y„  zj  and  perpendicular  distances  p„  p2 . pc  from  the  plane.  Then  we  shall  make 


U  =  X(p2)  *  a  minimum. 


Let  x  =  L(x,)/n.  v  =  £(x,)/n  and  z  =  £(x,)/n 


(1) 
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be  the  mean  values  of  the  variables; 

cr,2  =  X(x,2)/n  -  x2,  oy2  =  X(y2)/n  -  f  and  a,2  =  X(z,2)/n  -  r  (2) 

be  the  standard-deviations,  and,  lastly,  let 

r,y  =  T(xv)  -  nxv.  r„  =  X(x. z)  -  nxz  and  ryJ  =  X('vz)  -  nvz,  (3) 

no,oy  no,a!  n  a,  a, 

be  the  correlations  of  the  variables. 

Now  let  1,,  12  and  13  be  the  direction  cosines  of  a  plane 

l,x  +  l,y  +  l,z  =  p  (4) 

at  a  perpendicular  distance  p  from  the  origin.  We  shall  have 

l,2  +  122  +  132  =  1  (5) 

Then  we  require  to  make  a  minimum  of 

U  =  X(ljX  +  l:y  +  l3z  -  p)2  (6) 

by  variation  of  1,,  12,  13  and  p  subject  to  (5).  The  Lagrangian  multiplier  theorem  is  used  to 
minimize  U  under  the  constraint  (5).  To  use  this  theorem  a  Lagrangian  multiplier  Q  and  a 
function  U’  are  introduced: 

U*  =  X(l,x  +  Uy  +  l3z  -  p)2  -  Q(l,2  +  122  +  l,2  -  1) 

Differentiation  with  regard  to  p  gives: 

1,X(X.)  +  l:X(v,)  +  l,X(v,)  -  np  =  0 

or 

p  =  1.x  1J’  +  l,z  0) 

which  shows  us  that  the  best  fitting  plane  passes  through  the  centroid  of  the  system. 
Differentiation  with  regard  to  1„  1;  and  13  gives: 

9il  =  0  =  l,X(x2)  +  UHxy,)  *  l,X(x,z.)  -  pX(x,)  -  Ql; 

ais 

au  =  o  =  ... 

ai2 
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an  =  o  =  ... 

3i» 

substituting  for  p  from  (7)  gives: 

+  l2I(x,y,)  +  13I(X,Z,)  -  IxKx.)  -  LyX(x,)  -  l3zl(x,)  -  Ql,  =  0 
using  (2)  and  (3)  gives: 

1.0,*  +  l:o,o;x,y  +  l3c,ozrn  -  (Q/n)l.  =  0 

1,0,0^  +  l,oy2  +  1,0,0/^  -  (Q/n)lj  =  0  (8) 

1><W»  +  1,0,0^  +  l,o22  -  (Q/n)l,  =  0 
These  are  the  type  equations. 

We  can  proof  that  Q  =  Um  =  minimum  value  of  U.  Let  S  be  the  mean  square  of  the 
residuals: 


S  =  £fi-x  +  1-v  +  l,z  -  pi2 
n 


Then:  Q/n  =  S;  this  gives  a  physical  meaning  to  Q. 


The  determinantal  equations  of  the  type  equations: 


(c,2  -  S) 

(a, 2  -  S) 

<Wy ,, 

<w« 

(a,2  -  S) 

(1  -  S/c,2) 

r«5 

r» 

r* 

(1  -  S/c,2) 

fy. 

L, 

a  -  S/O, 

=  0 


We  must  choose  the  least  root  of  this  equation,  for  the  mean  square  residual  must  be  as 
small  as  possible.  Substitute  this  value  of  S  in  the  type  equations  and  we  find  the  required 
values  of  1,.  12  and  13  using  (5). 


A  least  squares  approximation  of  a  straight  line  can  be  done  in  a  similar  manner. 
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Defining  the  real  location  of  the  natural  frame  of  an  object  is  equivalent  to  defining  the 
transformation  matrix  T  of  that  frame  relative  to  the  world  frame.  As  stated  above,  we 
will  perform  point  measurements  on  features.  Up  to  this  moment,  in  defining  the  real 
location  of  natural  frames,  we  will  only  use  planes  and  straight  lines  as  features.  The 
measurements  will  be  performed  relative  to  the  world  frame,  so  they  will  give  us  the 
equations  of  the  planes  and  lines  relative  to  the  world  frame.  From  the  geometrical  model 
of  the  object  we  also  have  to  our  disposal  the  equations  relative  to  the  object's  natural 
frame. 

These  equations,  relative  to  the  world  frame  and  relative  to  the  object's  natural  frame,  form 
the  input  to  an  algorithm  that  will  calculate  the  transformation  mamx. 

Measuring  points  on  a  line  or  plane  in  an  arbitrary  way.  will  result  in  equations  without 
orientation  information.  If  we  need  orientation  information,  we  have  to  measure  the 
features  in  a  specific  way.  For  a  line,  the  first  and  last  point  will  define  the  orientation  of 
the  line;  for  a  plane,  the  last  point  will  be  a  point  above  the  plane  (above  means  outside 
the  object). 

So  we  have  the  following  equations: 

-  straight  line:  x  =  x„  r-a 
y  =  yp  +  r-b 
z  =  Zf  +  r-c 


or:  (\.  yr  z,)  and  sign-(a,  b,  c) 
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-  plane:  a^x  +  b  y  +  c  z  +  d  =  0 
or:  sign  (a,  b,  c,  d) 

With  sign  =  ±1  and  sign  will  only  be  known  if  we  have  orientation  information. 

In  the  following,  we  make  distinction  between  the  case  with  orientation  information  and  the 
case  without. 


4.1.  Features  with  orientation  information 

4.1.1.  Necessary  and  sufficient  information  to  define  T 

In  this  paragraph  we  define  the  minimum  number  of  features  we  have  to  measure. 

4. 1.1.1.  General 

-  in  the  following  a  is  a  vector  relative  to  the  world  frame  and  a’  a  vector  relative  to  the 
object’s  natural  frame 

-  we  divide  the  problem  in  finding  the  rotation  matrix  £  and  the  translation  vector  i 
with:  T  =  |R  1 1 

0  1_ 

-  for  R  we  need  2  direction  vectors 

proof:  *  given  a,  -  (a,„  aly,  au)T  and  a! 

h  =  (a*,,  a^,  a^)1  and  gl 
-  calculate:  j,  =  j.  x  j; 

fia  =  Si  X 


-  then: 

a2i  a3i 

al,  aL  a,' 

ai>  a2>  a}y 

=  &• 

ai>  a;,  a;y 

ai»  a2j  a‘H 

an  aii  a j. 

-  or:  A 

=  R-A' 

-  or:  ^ 

-  for  ;:  we  need  1  position  vector 

proof:  -  give;,  p  =  tp„.  p„  pj  and  p' 

-  then:  p  =  £p’  +  t 

-  or:  i  =  p  -  £-p’ 


4. 1.1. 2.  Points 

-  if  we  only  measure  points,  we  need  minimum  3  non-collinear  points 


10 


-  proof:  -  given:  a,  fe,  ^  and  a\  h\  c’ 

-  we  can  construct  two  direction  vectors:  ii  =  b  -  a 

r2  =  £  -  a 

so  we  can  find  R 

-  since  we  have  2  position  vectors,  we  can  find  I 


4. 1.1. 3.  Straight  lines 

-  if  we  only  measure  straight  lines,  we  need  minimum  2  non-parallel  straight  lines 


-  proof:  -  given:  -  line  1:  direction  vector  and  a  point  p,  on  the  line 

(and  also  a}  and  a  different  point  p’) 

-  line  2:  &  and  & 

-  we  have  2  direction  vectors,  so  we  can  find  R 

-  we  can  calculate  1  specific  point  on  one  of  the  lines  :  e.g.  the  point  on  line  1 
closest  to  line  2  (see  fig.);  so  we  can  find  I 


4. 1.1.4.  Planes 

-  if  we  only  measure  planes,  we  need  minimum  3  non-parallel  planes 

-  proof:  -  the  intersection  of  the  3  non-planar  planes  results  in  3  non-parallel  straight  lines, 

so  we  have  enough  information  (see  4. 1.1.3.) 


4. 1.1. 5.  Combination  of  straight  lines  and  planes 
a)  1  Straight  line  and  1  plane 

-  sufficient  if  the  straight  line  is  not  parallel  nor  perpendicular  to  the  plane 
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-  proof:  -  R:  take  direction  vector  of  the  straight  line  and  the  perpendicular  to  the  plane 

-  j:  calculate  intersecting  point  of  the  straight  line  with  the  plane 

-  because  the  2  conditions  on  the  straight  line  and  the  plane  are  somewhat  contradictors',  1 
straight  line  and  1  plane  are  sufficient,  but  the  algorithm  will  be  numerically  instable. 

b)  1  Line  and  2  planes 

-  sufficient  if  the  straight  line  is  non-parallel  to  the  intersection  of  the  planes 

-  proof:  -  with  the  straight  line  and  the  intersection  of  the  planes  we  have  two  straight 

lihes  (see  4.1. 1.3.) 


4.1.2.  Defining  T  with  3  or  more  points 

See  4.2.2. 


4.1.3.  Defining  T  with  3  or  more  planes 


-  given:  -  for  i  =  1,  n 

*  (q»,  q*,  qJ  from  plane  q*-x  +  q^-y  +  q^  z  +  qQ  =  0 
measured  relative  to  the  world  frame 
-  (a*,  a,,,  a*,  aj  from  plane  a»-x  +  a^-y  +  a^z  +  aD  =  0 
measured  relative  to  the  object  frame 


then: 

q»i  q»  q*i  q«i 

3»i  3*1  at. 

<1*  q*  q*  q^ 

— 

3»2  3y2 

q»  q^  q»  q™ 

3m  a^t  a,;,, 

(for  proof  see  Appendix  1) 


or:  £ 

>’T  = 

I  = 

Ei 

0  1 

SO  X"  = 

with  R  orthogonal,  so  RT-R  =  j 


ET  *ET-i 

o  1 


-  substituting  into  (9)  gives: 


q»;  q»;  4*i 

a,;  ay;  a,: 

q*:  q>2  q«2 

= 

3,2  3,2  a^ 

q.  q>»  q. 

3«1  a^ 

and 


(9) 


12 


qc. 

2*1  2yi  2,| 

rd 

Qc2 

2^2  2y2  a^ 

•RTI  + 

2,2 

Qcb 
-  . 

2xe  2^,  Ej, 

-  this  gives  two  sets  of  equations: 


P 

* 

q»i  • 

-  q» 

=  R- 

2»i 

2o  - 

a» 

Qy!  q>2 

q» 

2y» 

q.1  q.1  • 

•  q» 

b- 

2x2 

a» 

and 


q*. 

m  ■ 

q»i  qyi  q»i 

»  m 

tx 

2c, 

qc2 

s  - 

q*2  qy:  0x2 

• 

0 

+ 

2,2 

... 

... 

... 

q<= 

q»  Oyi]  q» 

*  «j 

• 

2cn 

L  . 

we  will  solve  the  two  sets  of  equations  separately 


(10) 


(11) 


4. 1.3.1.  Solving  for  R 

(10)  can  be  written  in  the  following  way: 


or  q,  =  R-a,  for  i  =  1,  n 

or  Q  =  R-A 

We  have  to  find  the  rotation  matrix  £  such  that  £TR  =  I 

Applying  least  squares  approximation  we  can  define  the  following  function  that  has  to  be 
minimized: 


f(ED  =  X(£a,  •  a.)T(&a,  -  a,) 

The  Lagrangian  multiplier  theorem  is  used  to  determine  the  matrix  R  that  minimizes  f 
under  the  constraint  condition  =  J.  To  use  this  theorem  a  (3x1)  vector  £  of 
Lagrangian  multipliers  and  a  function  F  of  &  and  £  are  introduced: 


F(fc,  S)  *  f(B)  +  ST-(ET-&  -  D  S. 


To  find  £  and  £  we  take  the  partial  derivatives  of  F  with  respect  to  &  and  £  and  equal 
them  to  zero. 
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3E  =  0  gives  RTR  =  I 

as 


3F  =  0  gives: 

as 

f  =  S(a,T-RT-£  s.)  -  I(a,T-RT-a,)  -  W-fra.)  +  &aT-a)  +  ST-(RT-R  -  I)  S 

aF  =  o  =  2-KK^gT)  -  Ka-a,1)  -  Ka.-O  +  o  +  2-cr-s-S7) 

aR 

2:R-I(a,-alT)  -  21(4-0  +  2-R-£-iT  =  0 

E-A  At  -  Q  At  +  K  S-St  =  o 

R-(A-AT  +  S-S1)  =  Q-AT  (12) 

with  =  £-AT  +  SST 

M  =  2AT 

(12)  becomes:  SI  =  E£’ 
with  RT  R  =  I  it  follow's  that: 


S’2  =  S’T-ET-ES’  =  MT-M 

MT-N1  is  a  symmetric  matrix  with  eigenvalues  Dn2  >  D22  >  D332  >  0  and  a  corresponding 
set  of  three  orthogonal  eigenvectors.  The  eigenvalues  are  arranged  on  the  principal 
diagonal  of  an  matrix  g2  while  the  eigenvectors  are  considered  as  the  columns  of  a  (3x3) 
matrix  V.  From  the  definition  of  eigenvalues  and  eigenvectors  it  is  seen  that: 

fcf-M  =  S’2  =  V^-V*  and  V-VT  =  J 

A  solution  for  the  symmetric  matrix  S’  is  therefore  given  by  S’  =  V-J)-VT,  where  the  signs 
of  the  principal  diagonal  components  D,lt  Da  and  D33  of  2  are  positive  (this  can  be 
proofed). 

Insertion  of  this  solution  into  SJ  =  £  S’  gives: 

M  =  E-X-E-X1 

or.  £-y-E  =  SI  X  =  jmi  mi  mj 

As  ^  is  a  diagonal  matrix,  column  1  and  column  2  of  £V  are  equal  to  (l/T>n)-mi  and 
(l/Dal-im  respectively.  The  third  column  of  ftV  follows  from  the  observation  that  £  V  is 
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orthogonal.  So  the  final  result  for  £  is  given  by: 

R  =  jci/Dn)iHi  (l/D^nij  (1/Dn-DjiHmjXrna) 
Concluding,  the  procedure  for  the  calibration  of  R  is  the  following: 
1)  determine  M  =  Q  Ar 

21  determine' the  eigenvalues  and  eigenvectors  of  MT-M 


•VT 


3)  calculate  nil  nk  of  M-V 

4)  calculate  £  according  to  (13). 


(13) 


4. 1.3.2.  Solving  for  l 


(11)  can  be  written  as: 


— 

r  -j 

- 

Ox!  9yl  9x1 

9 

aci  ”  9ci 

9x2  9y2  9x2 

• 

9 

ac2  *  9c2 

... 

9 

... 

9  XX  9yx  9 IX 

»  41 

acl  *  9x1 

- 

or:  QT-I  =  d 


This  is  an  overdetermined  set  of  equations:  we  can  solve  this  with  a  least  squares 
approximation.  The  normal  equations  are: 

(2-2t)-i  =  (£-d) 


so  i  =  (2-£T)-’-(2-d) 


The  solution  of  a  least  squares  problem  directly  from  the  normal  equations  is  rather 
susceptible  to  roundoff  error.  An  alternative,  and  preferred,  technique  involves  Singular 
Value  Decomposition,  the  benefits  of  which  we  may  investigate  in  the  future. 


4. 1.3.3.  Remark 

Another  possible  way  of  calculating  the  transformation  matrix  is  not  to  take  in 
consideration  the  orthogonality  constraint;  then  we  don’t  have  to  split  the  calculation  up 
into  two  steps,  we  can  calculate  J  in  one  step  with  a  least  squares  approximation. 

Starting  from  equation  (9)  we  have: 


Q’T  *  A’tT 
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Q’T-T  =  A’T 
r-Q’  =  & 

r  =  A’Q’^’Q’1)1 

I  =  (S’-Q’V-Q’-A’1 

The  same  remark  can  be  made  here,  that  it  is  better  to  use  Singular  Value  Decomposition. 

The  advantage  of  this  technique  is  that  the  algorithm  is  shorter  and  that  it  can  be  used  for 
4.1.2.  with  no  changes.  The  disadvantage  is  that  we  don’t  have  any  assurance  on  the 
orthogonality  of  Jl.  Therefore  research  should  be  performed  to  investigate  what  will 
happen  kinematically  if  we  represent  the  location  of  frames  by  a  transformation  matrix  with 
a  nearly  orthogonal  matrix  and  both  methods  (method  with  and  without  orthogonality 
assurance)  should  be  compared  by  an  accuracy  and  sensitivity  analysis.  We  can  do  this  in 
the  following  way: 

Accuracy  analysis 

-  generate  with  a  random  number  generator  (RNG)  a  translation  vector  and  3  Euler  angles 
out  of  which  we  can  calculate  an  orthogonal  matrix  &  this  gives  us  a  transformation 
matrix  T 

-  generate  with  the  RNG  the  equations  of  5  planes;  this  provides  us  with  Q' 

-  transform  Q’  into  A’ 

-  with  £  and  Q\  use  both  methods  to  define  X. 

-  compare:  (t.,,  -  tiJH0'< 

itijl 

-  check  orthogonality:  calculate  R,T-R, 


Sensititv  analysis 

-  calculate  I,  A’  and  2’  as  in  the  accuracy  analysis 

-  introduce  a  relative  error  E  in  Q’  with  0.1%  <  E  <  1.0%,  so  Qi  =  (1+E)-Q’ 
(E  will  be  generaicd  by  a  RNG) 

-  with  A’  and  Q,  use  both  methods  to  define  £ 

-  compare:  (t^  -  t,.)T0^ 

IM  ' 

-  check  orthogonality:  calculate  &T-&c 
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4.1.4.  Defining  T  with  2  straight  lines 


-  construct  3  planes:  •  containing  line  1  and  perpendicular  to  line  2 

•  containing  line  2  and  perpendicular  to  line  1 

•  containing  line  1  and  parallel  to  line  2 

-  remark:  the  two  lines  may  be  intersecting 


4.1.5.  Defining  T  with  more  than  2  lines  or  with  a  combination  of  lines  and  planes 

Reduce  all  the  information  to  planes  as  in  4.1.4. 

Define  T  following  4.1.3. 


4.2.  Features  without  orientation  information 
4.2.1.  Necessary  and  sufficient  information 

If  we  only  have  features  without  orientation  information,  we  will  compute  intersection 
points  between  the  different  features  and  we  will  define  X  with  this  intersection  points. 
Following  4. 1.1. 2  we  need  a  minimum  number  of  3  non-collinear  points  to  define  J. 

-  4  non-parallel  planes  will  yield  3  non-collinear  intersection  points 

-  under  the  following  conditions  (see  fig.)  3  straight  lines  will  yield  at  least  3  non- 
collinear  points 


PI 
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-  2  of  the  lines  have  to  be  scew 

-  the  third  line  may  be  any  line  except  the  line  through  PI  parallel  to  L2  and  the  line 
through  P2  parallel  to  LI 


4.2.2.  Defining  T  with  3  or  more  points 

First  we  have  to  compute  at  least  3  intersection  points.  These  means  that  we  need 
algorithms  to'  compute  the  following: 

-  the  intersection  point  between  two  intersecting  lines 

-  the  intersection  point  between  a  line  and  a  plane 

-  the  intersecting  line  between  two  planes 

-  the  two  closest  points  between  two  scew  lines 

The  algorithm  to  define  X  with  3  or  more  points  is  similar  to  that  one  with  three  or  more 
planes  (see  4.1.3.).  It  is  based  on  the  same  concepts,  so  we  will  not  explain  the  algorithm 
in  detail  here. 


5.  Auxiliary  frames  and  condition  types 

The  previous  paragraph  dealt  with  the  first  part  of  the  world  model  calibration:  defining  the 
real  locations  of  the  natural  frames  in  the  world  model.  This  first  pan  eliminates  or 
reduces  the  errors  caused  by  the  introduction  of  the  objects  into  the  work  environment. 

The  second  pan  of  the  world  model  calibration  deals  with  the  objects  themselves.  It 
consists  in  defining  the  real  locations  of  auxiliary  frames.  These  auxiliary  frames  represent 
special  characteristics  of  the  objects.  In  defining  the  real  locations  of  auxiliary  frames,  we 
will  also  use  circle  features. 

As  mentioned  in  chapter  2,  to  define  the  real  locations  of  the  frames,  we  will  connect  the 
frames  to  features  by  imposing  a  condition  on  the  frame. 

A  natural  frame  will  be  connected  to  several  features  and  can  as  such  be  conditioned  by  2 
condition  types: 

Cl:  determines  the  transformation  matrix  of  the  frame  without  considering  orientation 
C2:  determines  the  transformation  matrix  of  the  frame  with  considering  orientation 


An  auxiliary  frame  can  only  be  connected  to  1  feature  and  as  such  be  conditioned  by  the 
following  condion  types: 

C3:  imposes  the  origin  of  the  frame  to  lie  on  the  line  or  plane  feature 


C4:  imposes  the  X-axis  of  the  frame  to  be  perpendicular  to  the  plane  feature 

C5:  imposes  the  Y-axis  of  the  frame  to  be  perpendicular  to  the  plane  feature 

C6:  imposes  the  Z-axis  of  the  frame  to  be  perpendicular  to  the  plane  feature 

C7:  imposes  the  (X,Y)-plane  of  the  frame  to  be  perpendicular  to  the  line  or  plane  feature 

C8:  imposes  the  (X,Z)-plane  of  the  frame  to  be  perpendicular  to  the  line  or  plane  feature 

C9:  imposes  the  (Y,Z)-plane  of  the  frame  to  be  perpendicular  to  the  line  or  plane  feature 

CIO:  imposes  the  (X,Y)-plane  of  the  frame  to  be  parallel  to  the  plane  feature 

Cll:  imposes  the  (X,Z)-plane  of  the  frame  to  be  parallel  to  the  plane  feature 

Cl 2:  imposes  the  (Y,Z)-plane  of  the  frame  to  be  parallel  to  the  plane  feature 

C13:  pointing  to  a  circle  feature,  imposes  the  following  conditions  on  a  frame: 

-  origin  of  the  frame  must  be  identical  to  the  center  of  the  circle 

-  (X,Y)-plane  of  the  frame  must  be  identical  to  the  plane  of  the  circle 

You  can  find  the  algorithm  for  Cl  and  C2  in  chapter  4. 

For  each  other  condition  (C3  -  Cl 3),  used  on  auxiliary  features,  we  have  to  develop  an 
algorithm  that  will  adjust  the  transformation  matrix  of  the  frame  to  the  condition  imposed 
by  the  feature. 

For  instance,  with  C3,  the  origin  of  the  frame  will  be  projected  perpendiculary  on  the 
feature  and  the  frame  will  be  translated  accordingly. 


6.  Example 

The  global  procedure  for  creating  a  robot  program,  including  calibration  of  the  world 
model  was  briefly  outlined  in  the  introduction.  For  a  better  understanding  of  the  third 
step,  namely  the  introduction  of  features  into  the  world  model  and  the  conditioning  of  the 
location  of  important  frames  upon  these  features,  we  will  give  a  short  example. 

The  following  figure  views  an  object  with  5  frames  attached  to  it.  Frame  1  is  the  natural 
frame  of  the  object.  Frame  2  to  5  are  important  auxiliary  frames  necessary  in  an  assembly 
operation. 

To  calibrate  this  model  we  will  introduce  six  features  into  the  model  and  condition  the 
location  of  the  frames  upon  these  features  according  to  the  following  condition  types: 
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-  frame  1  will  be  connected  to  features  1  to  5,  which  impose  condition  Cl  on  the  frame 

-  frame  2  will  be  connected  to  feature  2,  which  imposes  condition  02  on  the  frame 

-  frame  3  will  be  connected  to  feature  3,  which  imposes  condition  C6  on  the  frame 

-  frame  4  will  not  be  connected  to  a  feature,  but  it  is  connected  to  frame  3 

-  frame  5  will  be  connected  to  feature  6,  which  imposes  condition  13  on  the  frame 

After  measuring  the  6  features  the  location  of  the  5  frames  will  be  updated  according  to 
the  imposed  conditions. 
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Appendix  1 


If  a  point  v  = 


x 


y 

z 

1 


lies  in  a  plane  SR  =  [a  b  c  d]  then  the  matrix  product: 


SR-v  =  0 


(1) 


or  in  expanded  form: 


a-x  +  b-y  +  cz  +  d  =  0  (2) 

Let  T  be  a  transformation  matrix  representing  a  natural  frame  (X\  Y\  Z’)  of  an  object 
relative  to  the  world  frame  (X,  Y,  Z). 


Given  [x’  y’  z’  1]T  the  coordinates  of  a  point  relative  to  the  natural  frame  and  [x  y  z  1]T 
the  coordinates  of  that  same  point  relative  to  the  world  frame,  then: 


X 

m  « 

x’ 

y 

=  T- 

y’ 

z 

z’ 

.1 

1 

Given  [a’  b’  c’ d’]  the  row  matrix  of  a  plane  relative  to  the  natural  frame  and  [abed] 
the  row  matrix  of  the  same  plane  relative  to  the  world  frame,  then: 


[a  b  c  d]  =  [a’  b’  c’  d’M1 


(4) 


as  we  require  that: 


[a  b  c  d]- 

»  » 

X 

=  [a’  b’  c’ d’]- 

x’ 

y 

y* 

z 

z’ 

1 

»  ■ 

1 

(5) 


To  verify  this  we  substitute  from  (3)  and  (4)  into  the  left  hand  side  of  (5)  and  we  obtain: 


b’  c’  d’l-X'-r 

•  * 

♦ 

X 

=  [a’  b’  c’ d’]- 

M  « 

x’ 

y’ 

y* 

z’ 

z’ 

1_ 

\ 

If  we  have  more  than  one  plane,  we  can  easily  expand  (4)  into 


a]  b!  Cj  d; 

a;  b;  c;  df 

b2  C2  d, 

ai  b;  c’  dj 

5  b.  c0  d. 

a;  b’  c’ d; 

